#include "HCSR04.h"
#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "Servo.h"
#include "Motor.h"
#include "IR.h"
#include "Bluetooth.h"
#include "InfrTrackSensor.h"

uint16_t Timer;  //该变量用于计数，在外部通过定时器调用
static HCSR04_Flag Flag;
float Servo_Angle=90;

/**
  * @brief  HCSR04超声波测距初始化
  * @param  无
  * @retval 无
  */
void HCSR04_Init()
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
	
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);  //关闭JTAG调试端口，使其变为普通的IO口（引脚不足使用）
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;  //用于输出触发信号
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPD;  //用于接收反馈信号，默认低电平
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_4;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	
	GPIO_ResetBits(GPIOB,GPIO_Pin_3);  //默认低电平
}

void HCSR04_Start()
{
	/*发送一个触发信号*/
	GPIO_SetBits(GPIOB,GPIO_Pin_3);
	Delay_us(15);
	GPIO_ResetBits(GPIOB,GPIO_Pin_3);
	
	uint16_t i=10000,j=10000;
	while(i--)
	{
		if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4)==1)break;
		Timer=0;  //计数清0并开始计数，每隔0.1ms计数加1
	}
	while(j--)
	{
		if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4)==0)break;
	}
}

/**
  * @brief  获取距离值
  * @param  无
  * @retval 距离值
  */
uint8_t HCSR04_GetDistace()
{
	uint8_t Distance;
	
	HCSR04_Start();
	Distance=Timer*341/2/100;  // cm(距离)
	Delay_ms(20);
	
	return Distance;
}

/**
  * @brief  获取标志位
  * @param  HC_Flag：要设定的标志位，参数可以是：HC_CHECK_FLAG，HC_CTRCAR_FLAG
  * @retval 标志位的值
  */
uint8_t HCSR04_GetFlag(uint8_t HC_Flag)
{
	assert_param(HC_Flag==HC_CHECK_FLAG||HC_Flag==HC_CTRCAR_FLAG);
	
	if(HC_Flag==HC_CHECK_FLAG)
	{
		return Flag.HCSR04_CheckFlag;
	}
	else if(HC_Flag==HC_CTRCAR_FLAG)
	{
		return Flag.HCSR04_CtrCarFlag;
	}
	return 0;
}

/**
  * @brief  设定标志位
  * @param  HC_Flag：要设定的标志位，参数可以是：HC_CHECK_FLAG，HC_CTRCAR_FLAG
  * @param  Value：要设定的值，该值可以是：HC_RESET，HC_SET
  * @retval 无
  */
void HCSR04_SetFlag(uint8_t HC_Flag,HC_SetValue Value)
{
	if(HC_Flag==HC_CHECK_FLAG)
	{
		Flag.HCSR04_CheckFlag=Value;
	}
	else if(HC_Flag==HC_CTRCAR_FLAG)
	{
		Flag.HCSR04_CtrCarFlag=Value;
	}
}

/**
  * @brief  获取舵机角度值
  * @param  无
  * @retval 角度值
  */
float HCSR04_GetServoAngle()
{
	return Servo_Angle;
}

/**
  * @brief  HCSR04控制小车
  * @param  无
  * @retval 无
  */
void HCSR04_CtrMasterCar(SetMotor_Speed* HC_Speed)
{
	static uint8_t HC_State=0;
//	if(HCSR04_GetDistace()<=40&&IR_GetFlag(IR_CTRCAR_FLAG)!=1&&Bluetooth_GetFlag(BLUE_CTRCAR_FLAG)!=1)
//	{
//		Flag.HCSR04_CheckFlag=1;
//		Flag.HCSR04_CtrCarFlag=0;
//		/*检测到有障碍物时，电机停转，并寻找无障碍的方向*/
//		HC_Speed->Speed1=0;
//		HC_Speed->Speed2=0;
//		HC_Speed->Speed3=0;
//		HC_Speed->Speed4=0;
//		
//		if(Servo_Angle>=30&&Servo_Angle<=90)  //向右寻找
//		{
//			Servo_Angle-=30;
//		}
//		else if(Servo_Angle<30)
//		{
//			Servo_Angle=0;
//		}
//		if(Servo_Angle==0)
//		{
//			Servo_Angle=120;
//		}
//		
//		if(Servo_Angle>90&&Servo_Angle<=150)  //向左寻找
//		{
//			Servo_Angle+=30;
//		}
//		if(Servo_Angle>150)
//		{
//			Servo_Angle=180;
//		}
//		if(Servo_Angle==180)
//		{
//			Servo_Angle=90;
//		}
//		Servo_SetAngle(Servo_Angle);
////		Delay_ms(500);
//	}
//	/*检测到障碍物时，将电机方向转到正确方向*/
	if(HCSR04_GetDistace()<35&&IR_GetFlag(IR_CTRCAR_FLAG)!=1&&Bluetooth_GetFlag(BLUE_CTRCAR_FLAG)!=1)
	{
		/*通过标志位判断处于的不同状态*/
		Flag.HCSR04_CtrCarFlag=0;
		Flag.HCSR04_CheckFlag=1;
		/*先判断是否向右边*/
//		if(Servo_Angle==90)
//		{
//			HC_Speed->Speed1=60;
//			HC_Speed->Speed2=60;			//正前方无障碍直行
//			HC_Speed->Speed3=60;
//			HC_Speed->Speed4=60;
//		}
//		if(Servo_Angle<90&&Servo_Angle>=60)     //随着角度增大，转向力度逐渐增大
//		{
//			HC_Speed->Speed1+=10;  
//			HC_Speed->Speed2-=10;
//		}
//		if(Servo_Angle<60&&Servo_Angle>=30)
//		{
//			HC_Speed->Speed1+=15;
//			HC_Speed->Speed2-=15;		
//		}
//		if(Servo_Angle<30&&Servo_Angle>=0)
//		{
//			HC_Speed->Speed1+=20;
//			HC_Speed->Speed2-=20;	
//		}
//		/*判断是否向左边*/
//		if(Servo_Angle>90&&Servo_Angle<=120)
//		{
//			HC_Speed->Speed1-=10;
//			HC_Speed->Speed2+=10;
//		}
//		if(Servo_Angle>120&&Servo_Angle<=150)
//		{
//			HC_Speed->Speed1-=15;
//			HC_Speed->Speed2+=15;
//		}
//		if(Servo_Angle>150&&Servo_Angle<=180)
//		{
//			HC_Speed->Speed1-=20;
//			HC_Speed->Speed2+=20;		
//		}
		/*右转*/
		if(HC_State==0)
		{			
			HC_Speed->Speed1=100;
			HC_Speed->Speed2=-100;
			HC_Speed->Speed3=100;
			HC_Speed->Speed4=-100;
			
//			HC_State=1;
		}
//		else if(HC_State==1)
//		{	
//			HC_Speed->Speed1=-100;
//			HC_Speed->Speed2=100;
//			HC_Speed->Speed3=-100;
//			HC_Speed->Speed4=100;
//			
//			HC_State=0;
//		}		
	}
	/*未检测到障碍物并且蓝牙，红外遥控和红外寻迹都未开启*/
	else if(HCSR04_GetDistace()>=35&&IR_GetFlag(IR_CTRCAR_FLAG)!=1&&Bluetooth_GetFlag(BLUE_CTRCAR_FLAG)!=1&&Infr_GetFlag()!=1)
	{
		HC_State=0;
		
		Flag.HCSR04_CtrCarFlag=1;
		Flag.HCSR04_CheckFlag=0;
			
		HC_Speed->Speed1=80;
		HC_Speed->Speed2=80;			//正前方无障碍直行
		HC_Speed->Speed3=80;
		HC_Speed->Speed4=80;		
	}
	Motor_SetAllSpeed(HC_Speed);
//	Delay_ms(500);
//	Servo_Angle=90;
//	Servo_SetAngle(90);
}
